//
// Created by shine on 2020/3/20.
//
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>

#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/integral_image_normal.h>

ros::Publisher pub_AllPointCloud,pub_back,pub_left,pub_right;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_back(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_left(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_right(new pcl::PointCloud<pcl::PointXYZ>);

bool left_ok = false;
bool right_ok = false;
bool back_ok = false;
bool front_ok = false;



void cloud_left_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
    if(!left_ok) {
        pcl::PCLPointCloud2 pcl_pc2_temp;
        pcl_conversions::toPCL(*cloud_msg, pcl_pc2_temp);
        cloud_in_left->clear();
        pcl::fromPCLPointCloud2(pcl_pc2_temp, *cloud_in_left);

        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(cloud_in_left);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(0, 10);
        pass.setKeepOrganized(true);
        //pass.setFilterLimitsNegative (true);
        pass.filter(*cloud_in_left);

        // Create the filtering object
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud(cloud_in_left);
        sor.setLeafSize(0.01f, 0.01f, 0.01f);
        sor.filter(*cloud_in_left);

        left_ok = true;
    }
}

void cloud_right_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
    if(!right_ok){
        pcl::PCLPointCloud2 pcl_pc2_temp;
        pcl_conversions::toPCL(*cloud_msg, pcl_pc2_temp);
        cloud_in_right->clear();
        pcl::fromPCLPointCloud2(pcl_pc2_temp, *cloud_in_right);

        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(cloud_in_right);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(0, 10);
        pass.setKeepOrganized(true);
        //pass.setFilterLimitsNegative (true);
        pass.filter(*cloud_in_right);

        // Create the filtering object
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud(cloud_in_right);
        sor.setLeafSize(0.01f, 0.01f, 0.01f);
        sor.filter(*cloud_in_right);

        right_ok = true;
    }
}

void cloud_back_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {

    if(!back_ok){
        pcl::PCLPointCloud2 pcl_pc2_temp;
        pcl_conversions::toPCL(*cloud_msg, pcl_pc2_temp);
        cloud_in_back->clear();
        pcl::fromPCLPointCloud2(pcl_pc2_temp, *cloud_in_back);

        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(cloud_in_back);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(0, 10);
        pass.setKeepOrganized(true);
        //pass.setFilterLimitsNegative (true);
        pass.filter(*cloud_in_back);

        // Create the filtering object
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud(cloud_in_back);
        sor.setLeafSize(0.01f, 0.01f, 0.01f);
        sor.filter(*cloud_in_back);

        back_ok = true;
    }
}

void cloud_front_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){

    if(left_ok&&right_ok&&back_ok){
        pcl::PCLPointCloud2 pcl_pc2_temp;
        pcl_conversions::toPCL(*cloud_msg,pcl_pc2_temp);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_front(new pcl::PointCloud<pcl::PointXYZ>);
        cloud_in_front->clear();
        pcl::fromPCLPointCloud2(pcl_pc2_temp,*cloud_in_front);

        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(cloud_in_front);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(0, 10);
        pass.setKeepOrganized(true);
        //pass.setFilterLimitsNegative (true);
        pass.filter(*cloud_in_front);



///点云合并，但点云无序化
//        Eigen::Affine3f transform_left = Eigen::Affine3f::Identity();
//        Eigen::Affine3f transform_right = Eigen::Affine3f::Identity();
//        Eigen::Affine3f transform_back = Eigen::Affine3f::Identity();
////        transform.translation() << 0.0, 0.0, 0.0;
//        //旋转并合并右侧点云
//        transform_right.rotate (Eigen::AngleAxisf (M_PI/2, Eigen::Vector3f::UnitY()));
//        pcl::transformPointCloud (*cloud_in_right, *cloud_in_right, transform_right);
//        *cloud_in_front = *cloud_in_front + *cloud_in_right;
//        //旋转并合并左侧点云
//        transform_left.rotate (Eigen::AngleAxisf (-M_PI/2, Eigen::Vector3f::UnitY()));
//        pcl::transformPointCloud (*cloud_in_left, *cloud_in_left, transform_left);
//        *cloud_in_front = *cloud_in_front + *cloud_in_left;
//        //旋转并合并后侧点云
//        transform_back.rotate (Eigen::AngleAxisf (M_PI, Eigen::Vector3f::UnitY()));
//        pcl::transformPointCloud (*cloud_in_back, *cloud_in_back, transform_back);
//        *cloud_in_front = *cloud_in_front + *cloud_in_back;
//        left_ok = false;right_ok = false;back_ok = false;
//        std::cerr<<"Points cloud_in: "<<cloud_in_front->size()<<std::endl;

/*
///有序点云的法线估计
        pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
        pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
//        computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane_parameters, float &curvature);


 //三种法线估计方法
 //COVARIANCE_MATRIX 模式从具体某个点的局部邻域的协方差矩阵创建9个积分，来计算这个点的法线
 //AVERAGE_3D_GRADIENT   模式创建6个积分图来计算水平方向和垂直方向的平滑后的三维梯度并使用两个梯度间的向量积计算法线
//AVERAGE_DEPTH——CHANGE  模式只创建了一个单一的积分图，从而平局深度变化计算法线
        ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);  //设置法线估计的方式
        ne.setMaxDepthChangeFactor(3.0f);   //设置深度变化系数
        ne.setNormalSmoothingSize(20.0f);   //设置法线优化时考虑的邻域的大小
        ne.setInputCloud(cloud_in_front);               //输入的点云
        ne.compute(*normals);                    //计算法线
        // 可视化
        pcl::visualization::PCLVisualizer viewer("NormalEstimation");
        viewer.setBackgroundColor(0, 0, 0);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloudHandler
                (cloud_in_front, 255, 0, 0);
        viewer.addPointCloud(cloud_in_front, cloudHandler, "cloud_Normal");
        viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_in_front, normals);
        std::cout << "法向估计完成！" << std::endl;
        std::cout << cloud_in_front->size() << std::endl;
        std::cout << normals->size() << std::endl;
        while (!viewer.wasStopped())
        {
            viewer.spinOnce();
        }
*/

//        pcl::PassThrough<pcl::PointXYZ> pass;
//        pass.setInputCloud(cloud_in_front);
//        pass.setFilterFieldName("y");
//        pass.setFilterLimits(FLT_MIN, FLT_MAX);
//        pass.setKeepOrganized(true);
//        //pass.setFilterLimitsNegative (true);
//        pass.filter(*cloud_in_front);

///无序点云法向量估计
/*
        pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
        ne.setNumberOfThreads(12);  // 手动设置线程数，否则提示错误
        ne.setInputCloud (cloud_in_front);
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
        ne.setSearchMethod (tree);
        // Output datasets
        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
        // Use all neighbors in a sphere of radius 50cm
        ne.setRadiusSearch (0.5);
        ne.setViewPoint(0,0,0);
        // Compute the features
        ne.compute (*cloud_normals);

        pcl::visualization::PCLVisualizer viewer("NormalEstimation");
        viewer.setBackgroundColor(0, 0, 0);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloudHandler
                (cloud_in_front, 255, 0, 0);
        viewer.addPointCloud(cloud_in_front, cloudHandler, "cloud_Normal");
        viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_in_front, cloud_normals);
        std::cout << "法向估计完成！" << std::endl;
        std::cout << cloud_in_front->size() << std::endl;
        std::cout << cloud_normals->size() << std::endl;
        while (!viewer.wasStopped())
        {
            viewer.spinOnce();
        }
*/



///转为rosmsg
        sensor_msgs::PointCloud2 cloud_out;//转换为ros格式
        pcl::toROSMsg(*cloud_in_front,cloud_out);
        pub_AllPointCloud.publish(cloud_out);
    }


//    pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);//用来存放边界
//    std::cout<<boundingbox_ptr->size()<<std::endl;
//
//    boundingbox_ptr->push_back(pcl::PointXYZ(0.1,0.1-10,0));
//    std::cout<<boundingbox_ptr->size()<<std::endl;
//
//    boundingbox_ptr->push_back(pcl::PointXYZ(0.1,-0.1-10,0));
//    std::cout<<boundingbox_ptr->size()<<std::endl;
//
//    boundingbox_ptr->push_back(pcl::PointXYZ(-0.1,0.1-10,0.1));
//    std::cout<<boundingbox_ptr->size()<<std::endl;
//
//    std::cerr<<"--------------------------------------"<<std::endl;
//
//    pcl::ConvexHull<pcl::PointXYZ> hull;
//    hull.setInputCloud(boundingbox_ptr);
//    hull.setDimension(2);
//    std::vector<pcl::Vertices> polygons;
//    pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZ>);
//    hull.reconstruct(*surface_hull,polygons);
//
//    pcl::PointCloud<pcl::PointXYZ>::Ptr objects(new pcl::PointCloud<pcl::PointXYZ>);
//    pcl::CropHull<pcl::PointXYZ> bb_filter;
//    bb_filter.setDim(2);
//    bb_filter.setInputCloud(cloud_in);
//    bb_filter.setHullIndices(polygons);
//    bb_filter.setHullCloud(surface_hull);
//    bb_filter.filter(*objects);
//
////    pcl::io::savePCDFileASCII("outputPCL",*objects);
//

//    std::cerr<<"Points objects: "<<objects->size()<<std::endl;


//    std::cerr<<"Points objects: "<<objects->size()<<std::endl;



//    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
//    viewer->initCameraParameters();
//
//    int v1(0);
//    viewer->createViewPort(0,0,0.33,1,v1);
//    viewer->setBackgroundColor(255,255,255,v1);
//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color1(cloud_in,255,0,0);
//    viewer->addPointCloud(cloud_in,color1,"cloud_in",v1);
//    //viewer->addPolygon<pcl::PointXYZ>(cloud_in,0,255,0,"polyline1",v1);//添加一条边界线
//
//    int v2(0);
//    viewer->createViewPort(0.33,0,0.66,1,v2);
//    viewer->setBackgroundColor(255,255,255,v2);
//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2(surface_hull,255,0,0);
//    viewer->addPointCloud(surface_hull,color2,"surface_hull",v2);
//
//    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"surface_hull",v2);//设置点的大小
//    viewer->addPolygon<pcl::PointXYZ>(surface_hull,0,0,255,"polyline2",v2);//添加表示输入点云的多边形（折线、全部连接）
//
//    int v3(0);
//    viewer->createViewPort(0.66,0,1,1,v3);
//    viewer->setBackgroundColor(255,255,255,v3);
//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color3(objects,255,0,0);
//    viewer->addPointCloud(objects,color3,"objects",v3);
//
//    viewer->addCoordinateSystem();
//    viewer->spin();

}

int main (int argc, char** argv)
{
    // Initialize ROS
    ros::init (argc, argv, "normalEstimate");//声明节点的名称
    ros::NodeHandle nh;

    // Create a ROS subscriber for the input point cloud
    // 为接受点云数据创建一个订阅节点
    ros::Subscriber sub_front = nh.subscribe<sensor_msgs::PointCloud2> ("cam1/points2", 1, cloud_front_cb);
    ros::Subscriber sub_back = nh.subscribe<sensor_msgs::PointCloud2> ("cam5/points2", 1, cloud_back_cb);
    ros::Subscriber sub_right = nh.subscribe<sensor_msgs::PointCloud2> ("cam7/points2", 1, cloud_right_cb);
    ros::Subscriber sub_left = nh.subscribe<sensor_msgs::PointCloud2> ("cam3/points2", 1, cloud_left_cb);
    // Create a ROS publisher for the output point cloud
    //创建ROS的发布节点
    pub_AllPointCloud = nh.advertise<sensor_msgs::PointCloud2> ("AllPointCloudOutput", 1);
    // Spin
    ros::spin ();

//    ros::Rate loop_rate(5);
//    while (ros::ok())
//    {
//        /*...TODO...*/
//
//        ros::spinOnce();
//        loop_rate.sleep();
//    }

}
